#!/usr/bin/python3
from enum import Enum
from math import ceil

import serial
import time
from protocol_payload import DeviceStatus


# serialHandle = serial.Serial("com6", 115200)  # 初始化串口， 波特率为115200


class CtlAction(Enum):
    MOVE_TO = "MOVE_TO"
    MOVE = "MOVE"
    READ_POSITION = "READ_POSITION"
    REGISTER = "REGISTER"
    HEARTBEAT = 'HEARTBEAT'


command = {"MOVE_WRITE": 1, "POS_READ": 28, "LOAD_UNLOAD_WRITE": 31}

interval_position = 250
max_position = 1000
min_position = 0
max_runtime = 10 * 1000
error_code = -1


class Device(object):

    def __init__(self, serial_port: str, baud_rate=115200, device_id=1):
        self.__serial_port = serial_port;
        self.__baud_rate = baud_rate
        self.__device_id = device_id
        self.__status = DeviceStatus.OFFLINE
        self.__serial_handle = None

    def connect(self):
        self.__serial_handle = serial.Serial(self.__serial_port, self.__baud_rate)  # 初始化串口， 波特率为115200
        if self.self_checking():
            self.__status = DeviceStatus.STOP

    def get_device_id(self):
        return self.__device_id

    def move(self, start, end):
        if not self.self_checking():
            return error_code
        if start > end:
            start, end = end, start;
        if start < min_position:
            start = min_position
        if end > max_position:
            end = max_position
        self.move_to_position(start);

        interval = end - start
        self.update_running_status()
        for i in range(1, ceil(interval / interval_position)):
            self.__send_cmd(command["MOVE_WRITE"], start + interval_position * i, max_runtime)
            time.sleep(max_runtime / 1000 + 0.1)
        self.update_stop_status()
        return self.read_position()

    def move_to_position(self, position):
        if not self.self_checking():
            return error_code

        self.update_running_status()
        runtime = 3000
        self.__send_cmd(command["MOVE_WRITE"], position, runtime)
        time.sleep(runtime / 1000 + 0.1)
        self.update_stop_status()
        return self.read_position()

    def update_running_status(self):
        self.__status = DeviceStatus.RUNNING

    def update_stop_status(self):
        self.__status = DeviceStatus.STOP

    def __send_cmd(self, cmd, position=None, runtime=None):
        """
        发送控制命令
        ## 协议头 0x55 0x55 (协议头不计入len 长度)
        ## 1 byte 是 舵机ID
        ## 2 byte 是 数据len
        ## 3 byte 是 命令 1 代表转动舵机
        ## 后面的位数代表 第三byte位的参数
        :param cmd:
        :param position: 要运行到的位置
        :param runtime: 运行时间
        :return:
        """
        if not self.self_checking():
            return error_code
        buf = bytearray(b'\x55\x55')
        len = 3  # 若命令是没有参数的话数据长度就是3
        buf1 = bytearray(b'')

        ## 对参数进行处理
        if position is not None:
            len += 2  # 数据长度加2
            buf1.extend([(0xff & position), (0xff & (position >> 8))])  # 分低8位 高8位 放入缓存
            ## 1111 1111&0000 0001 = 0000 0001, 0000 0000
        if runtime is not None:
            len += 2
            buf1.extend([(0xff & runtime), (0xff & (runtime >> 8))])  # 分低8位 高8位 放入缓存

        buf.extend([(0xff & self.__device_id), (0xff & len), (0xff & cmd)])

        buf.extend(buf1)  # 追加参数

        ##计算校验和
        sum = 0x00
        for b in buf:  # 求和
            sum += b
        sum = sum - 0x55 - 0x55  # 去掉命令开头的两个 0x55
        sum = ~sum  # 取反
        buf.append(0xff & sum)  # 取低8位追加进缓存
        # print("buf == ", buf.hex())
        self.__serial_handle.write(buf)  # 发送

    def read_position(self):
        """
        读取设备当前位置
        :return number
        """
        if not self.self_checking():
            return -1
        self.__serial_handle.flushInput()  # 清空接收缓存
        self.__send_cmd(command["POS_READ"])  # 发送读取位置命令
        time.sleep(0.00034)  # 小延时，等命令发送完毕。不知道是否能进行这么精确的延时的，但是修改这个值的确实会产生影响。
        # 实验测试调到这个值的时候效果最好
        time.sleep(0.004)  # 稍作延时，等待接收完毕
        count = self.__serial_handle.inWaiting()  # 获取接收缓存中的字节数
        pos = None
        if count != 0:  # 如果接收到的数据不空
            recv_data = self.__serial_handle.read(count)  # 读取接收到的数据
            if count == 8:  # 如果接收到的数据是8个字节（符合位置读取命令的返回数据的长度）
                if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[4] == 0x1C:
                    # print("id = ", recv_data[2])
                    # print("len = ", recv_data[3])

                    # 第一第二个字节等于0x55, 第5个字节是0x1C 就是 28 就是 位置读取命令的命令号
                    pos = 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8)))  # 将接收到的字节数据拼接成完整的位置数据
                    # 上面这小段代码我们简化了操作没有进行和校验，只要帧头和命令对了就认为数据无误

        return pos  # 返回读取到的位置

    def self_checking(self):
        if self.__serial_handle is None:
            return False
        try:
            self.__serial_handle.write(b"HELLO WORLD\r\n")
            return True
        except Exception as e:
            print(e)
            return False

    def status(self):
        self.__status = DeviceStatus.STOP if self.self_checking else DeviceStatus.OFFLINE
        return self.__status
